clear;clc;close all
addpath('./util');

%% 读取光片图像
LS = loadtiff('data/LS.tif');
%LS =loadtiff('G:\20251103\SP\LS\1\Image_1_51.tif');
%LS = apply_window(LS, 224, 803);
%figure;sliceViewer(LS,[]);
size(LS)

%下采样z，旋转，padding
LS = rot90(LS, 1);
LS = flip(LS, 2);

%% 读取光场图像
LF = loadtiff('data/LF.tif');
size(LF)

%% 对准xy

im_moving = squeeze(max(LS(:,:,20-10:20+10), [], 3));
im_fixed  = squeeze(max(LF(:,:,20), [], 3));

im_moving_adjust = imadjust(im_moving);
im_fixed_adjust = imadjust(im_fixed);

%手工粗配准
[moving_points,fixed_points] = cpselect(im_moving_adjust,im_fixed_adjust,'Wait',true);
t_concord = fitgeotrans(moving_points,fixed_points,'similarity');
fixed_ref = imref2d(size(im_fixed)); %relate intrinsic and world coordinates
moving_registered = imwarp(im_moving,t_concord,'OutputView',fixed_ref);

figure, imshowpair(imadjust(moving_registered),imadjust(im_fixed),'falsecolor'); title('人工配准结果');

%提高配准精度
[optimizer, metric] = imregconfig('multimodal');
optimizer.InitialRadius = optimizer.InitialRadius*2;
optimizer.MaximumIterations = 300;

movingRegisteredAdjustedInitialRadius = imregister(im_moving, im_fixed, 'affine', optimizer, metric, ...
    'InitialTransformation',t_concord);

% 找到xy的变换
tform_xy = imregtform(im_moving, im_fixed, 'affine', optimizer, metric, ...
    'InitialTransformation',t_concord);

figure, imshowpair(imadjust(movingRegisteredAdjustedInitialRadius), imadjust(im_fixed),'falsecolor'); title('精细配准结果');

% 保存XY配准结果
save('./run/M20_2Dtransfrom.mat', 'fixed_points','tform_xy');


%% 将LS，xy变换到LF

fixed_ref = imref2d([size(LF, 1), size(LF,2)]);
tLStiff_M20_beads = transform_LS2LF(LS, tform_xy, fixed_ref);


%% 展示XY配准效果
im_moving = squeeze(max(tLStiff_M20_beads(:,:,:), [], 3));
im_fixed  = squeeze(max(LF(:,:,[1:30 70:101]), [], 3));
im_moving_adjust = imadjust(im_moving);
im_fixed_adjust = imadjust(im_fixed);

figure, imshowpair(im_moving_adjust, im_fixed_adjust,'falsecolor');  axis image;

%% 计算Z轴
%初始化
t = [tform_xy.T(1,1), tform_xy.T(1,2), 0      0;
    tform_xy.T(2,1), tform_xy.T(2,2),  0      0;
    0                0                 1      0;  %p(1)
    tform_xy.T(3,1), tform_xy.T(3,2),  0      1]; %p(2)


for iter = 1:1

    disp(['iter: ' num2str(iter)]);
    tform = affine3d(t); % 初始化
    fixed_ref3d = imref3d(size(LF));
    tLStiff_M20_beads = imwarp(LS,tform, 'OutputView', fixed_ref3d);

    %对准z
    %1. 找到光片中的beads
    [BW_LS, Y_LS, X_LS, Z_LS] = detect_beads_LS_3(tLStiff_M20_beads);
    xx_ls = X_LS';
    yy_ls = Y_LS';
    zz_ls = Z_LS';


    figure, imshow(imadjust(max(LF(:,:,:), [], 3)),[]),title('LF beads xy');
    for i = 1:size(yy_lf,2)
        hold on, plot(xx_lf(i), yy_lf(i), 'o', 'MarkerSize', 5);
    end
    figure, imshow(imadjust(squeeze(max(LF(:,:,:), [], 2))),[]), title('LF beads yz'); axis fill;
    for i = 1:size(yy_lf,2)
        hold on, plot(zz_lf(i), yy_lf(i), 'o', 'MarkerSize', 5);
    end

    % 2. 找到光场中的beads
    mLF=LF;
    mLF(:,:,40:60)= zeros(size(LF,1),size(LF,2),21);
    [BW_LF, Y_LF, X_LF, Z_LF] = detect_beads_LS_3(mLF);
    xx_lf = X_LF';
    yy_lf = Y_LF';
    zz_lf = Z_LF';

    figure, imshow(imadjust(max(LF(:,:,:), [], 3)),[]),title('LF beads xy');
    for i = 1:size(yy_lf,2)
        hold on, plot(xx_lf(i), yy_lf(i), 'o', 'MarkerSize', 5);
    end
    figure, imshow(imadjust(squeeze(max(LF(:,:,:), [], 2))),[]), title('LF beads yz'); axis fill;
    for i = 1:size(yy_lf,2)
        hold on, plot(zz_lf(i), yy_lf(i), 'o', 'MarkerSize', 5);
    end


    %遍历LF中的每个beads，在LS中寻找xy距离最小的beads
    clear lf_match_beads
    lf_match_beads=zeros(length(xx_lf),2);
    for i = 1:length(xx_lf)
        distance = sqrt((xx_lf(i) - xx_ls).^2 + (yy_lf(i) - yy_ls).^2);
        [sort_dis, index] = sort(distance);
        lf_match_beads(i,1) = sort_dis(1);% 记录下来最近的xy距离上的距离
        lf_match_beads(i,2) = index(1); % 记录下来最近的光片beads的索引
    end
    %找到xy距离小于10个像素   Z 方向上的相对距离在范围 [-8, 8] 内     ' 是转置符号，用于将列向量转为行向量，确保逻辑运算中的维度一致性
    I = find((lf_match_beads(:,1) < 10)  & ( (zz_lf - zz_ls(lf_match_beads(:,2))) < 8 )' &( (zz_lf - zz_ls(lf_match_beads(:,2))) > -8 )'); % xy距离小于10,z 上距离小于12

    %对找到的beads的z轴坐标进行线性拟合
    fit_xx=zz_ls(lf_match_beads(I,2))-1;
    fit_yy=zz_lf(I)-1;
    p = polyfit(fit_xx,fit_yy,1);
    z_fit = polyval(p, fit_xx);

    figure;
    plot(fit_xx, fit_yy,'o' ,fit_xx ,z_fit ,'-');
    legend('data','linear fit');

    %整体变换
    t = [tform_xy.T(1,1), tform_xy.T(1,2),       0         0;
        tform_xy.T(2,1), tform_xy.T(2,2),        0         0;
        0                0                t(3,3)*p(1)      0;  %p(1)
        tform_xy.T(3,1), tform_xy.T(3,2), t(4,3)*p(1)+p(2) 1]; %p(2)
end

%% 应用3D变换
tform = affine3d(t);
fixed_ref3d = imref3d(size(LF));
LS_3D_transformed = imwarp(LS,tform, 'OutputView', fixed_ref3d);
tform.T

%% 展示3D配准效果

xz_LS = squeeze(max(LS_3D_transformed(800:1000, :, :), [], 1));
xz_LF = squeeze(max(LF(800:1000, :, :), [], 1));

figure,
imshowpair(imadjust(xz_LS), imadjust(xz_LF), 'falsecolor'); axis fill;
title('xz');

yz_LS = squeeze(max(LS_3D_transformed(:, 800:1000, :), [], 2));
yz_LF = squeeze(max(LF( :, 800:1000, :), [], 2));

figure,
imshowpair(imadjust(yz_LS), imadjust(yz_LF), 'falsecolor'); axis fill;
title('yz');

xy_LS = squeeze(max(LS_3D_transformed(:, :, :), [], 3));
xy_LF = squeeze(max(LF(:, :, :), [], 3));

figure,
imshowpair(imadjust(xy_LS), imadjust(xy_LF), 'falsecolor');
title('xy');


%% 保存3D配准结果
transform.tform=tform;
transform.ref3d=fixed_ref3d;
save('./run/M20_3Dtransfrom.mat', 'transform');